

#ifndef __COMM_MSG_NAV_STATE_H__
#define __COMM_MSG_NAV_STATE_H__

#include "comm/comm_topic_name.h"
#include "comm/comm_base.h"
#include <comm_msg/nav_state.h>
#include <ros/ros.h>
#include <iostream>
#include <list>
#include <vector>

using namespace std;

/**************************************************************************************
功能描述: 规划运动趋势
修改记录:
**************************************************************************************/
typedef enum {
    PLAN_MOVE_TREND_STOP                = 0,        /* 停止运动 */
    PLAN_MOVE_TREND_FORWARD             = 1,        /* 向前运动 */
    PLAN_MOVE_TREND_BACKWARD            = 2,        /* 向后运动 */
    PLAN_MOVE_TREND_FREE                = 3,        /* 自由运动 */
    PLAN_MOVE_TREND_LIFT_TRASH_BOX      = 4,        /* 抬升垃圾箱 */
    PLAN_MOVE_TREND_WITHDRAW_TRASH_BOX  = 5,        /* 收回垃圾箱 */
} PLAN_MOVE_TREND_ENUM;

/**************************************************************************************
功能描述: 导航状态
修改记录:
**************************************************************************************/
typedef struct NAV_STATE_STRU{
    NAV_STATE_STRU(void)
    {
        enPlanMoveTrend = PLAN_MOVE_TREND_STOP;
    }

    PLAN_MOVE_TREND_ENUM enPlanMoveTrend;
} NAV_STATE_STRU;

/**************************************************************************************
功能描述: 发布导航状态消息
修改记录:
**************************************************************************************/
class NAV_STATE_SENDER_C
{
public:
    NAV_STATE_SENDER_C(ros::NodeHandle &nh)
    {
        publisher_ = nh.advertise<comm_msg::nav_state>(TOPIC_NAME_NAV_STATE, 1);

        stLastNavState_.enPlanMoveTrend = PLAN_MOVE_TREND_STOP;
    }

    void Stop(void)
    {
        SendMsg(PLAN_MOVE_TREND_STOP);
    }

    void MoveForward(void)
    {
        SendMsg(PLAN_MOVE_TREND_FORWARD);
    }

    void MoveBackward(void)
    {
        SendMsg(PLAN_MOVE_TREND_BACKWARD);
    }

    void MoveFree(void)
    {
        SendMsg(PLAN_MOVE_TREND_FREE);
    }

    void Send(const NAV_STATE_STRU &stNavState)
    {
        SendMsg(stNavState.enPlanMoveTrend);
    }

private:
    ros::Publisher publisher_;
    NAV_STATE_STRU stLastNavState_;

    BOOL UptLastNavState(PLAN_MOVE_TREND_ENUM enTrend)
    {
        if (stLastNavState_.enPlanMoveTrend != enTrend) {
            stLastNavState_.enPlanMoveTrend = enTrend;
            return true;
        }

        return false;
    }

    void SendMsg(PLAN_MOVE_TREND_ENUM enTrend)
    {
        comm_msg::nav_state stMsg;

        stMsg.planMoveTrend   = (UINT08)enTrend;

        publisher_.publish(stMsg);

        /* 有变更时记录日志 */
        if (UptLastNavState(enTrend)) {
            ST_LOG_INFO("(stop %u, forward %u, backward %u, free %u) enTrend=%u.",
                        PLAN_MOVE_TREND_STOP,
                        PLAN_MOVE_TREND_FORWARD,
                        PLAN_MOVE_TREND_BACKWARD,
                        PLAN_MOVE_TREND_FREE,
                        stMsg.planMoveTrend);
        }
    }
};

/**************************************************************************************
功能描述: 接收导航状态消息
修改记录:
**************************************************************************************/
class NAV_STATE_RECEIVER_C
{
public:
    NAV_STATE_RECEIVER_C(ros::NodeHandle &nh)
    {
        subscriber_ = nh.subscribe(TOPIC_NAME_NAV_STATE, 1, &NAV_STATE_RECEIVER_C::ReceiveMsgCallBack, this);

        stNavState_.enPlanMoveTrend = PLAN_MOVE_TREND_STOP;
    }

    void Get(NAV_STATE_STRU &stNavState) const
    {
        stNavState = stNavState_;
    }

private:
    ros::Subscriber subscriber_;
    NAV_STATE_STRU  stNavState_;

    void ReceiveMsgCallBack(const comm_msg::nav_state::ConstPtr &pstMsg)
    {
        R_ASSERT(pstMsg != NULL, NOTHING);

        /* 以最新状态为准，所以直接覆盖老状态即可 */
        NAV_STATE_STRU stNewState;
        stNewState.enPlanMoveTrend = PLAN_MOVE_TREND_ENUM(pstMsg->planMoveTrend);

        if (stNewState.enPlanMoveTrend != stNavState_.enPlanMoveTrend) {
            ST_LOG_INFO("New Nav State: (stop %u, forward %u, backward %u, free %u) enTrend=%u.",
                        PLAN_MOVE_TREND_STOP,
                        PLAN_MOVE_TREND_FORWARD,
                        PLAN_MOVE_TREND_BACKWARD,
                        PLAN_MOVE_TREND_FREE,
                        stNewState.enPlanMoveTrend);

            stNavState_ = stNewState;
        }
    }
};


#endif

